from vpython import *

from pathlib import Path
import matplotlib.pyplot as plt
from concurrent.futures import ThreadPoolExecutor
from functools import partial
from typing import List, Dict, Tuple, Any, Union, Callable, Generator
from numpy.typing import DTypeLike

from pyclustering.utils.metric import type_metric, distance_metric
from pyclustering.cluster.center_initializer import kmeans_plusplus_initializer
from pyclustering.cluster.kmeans import kmeans
from pyclustering.cluster.kmedians import kmedians
from pyclustering.cluster.kmedoids import kmedoids

from sklearn.metrics import pairwise_distances
from sklearn.cluster import AgglomerativeClustering
from sklearn.cluster import DBSCAN, HDBSCAN
from sklearn.cluster import KMeans

from scipy.interpolate import CubicSpline
from scipy.linalg import eigh

import numpy as np
import random
import bisect
import math
import time
import yaml
import json


Distance_Func = Callable[
    [
        np.ndarray[Tuple[int], np.dtype[np.float64]],
        np.ndarray[Tuple[int], np.dtype[np.float64]],
    ],
    float,
]
Distance_Func_Combine = Callable[
    [Distance_Func, int, int],
    Distance_Func,
]


class quaternion:
    """
    四元数类
    """

    def __init__(self, w: float = 1.0, x: float = 0.0, y: float = 0.0, z: float = 0.0):
        """
        四元数
        :param w: 实部
        :param x: x 轴分量
        :param y: y 轴分量
        :param z: z 轴分量
        """
        self.w = w
        self.x = x
        self.y = y
        self.z = z

    @staticmethod
    def A_pose() -> "quaternion":
        """
        生成 A-Pose 四元数
        :return: A-Pose 四元数
        """
        return quaternion.euler(180, 180, 0)

    @staticmethod
    def euler(X: float, Y: float, Z: float, order: str = "ZXY") -> "quaternion":
        """
        从欧拉角构建四元数
        :param X: 绕 X 轴旋转角度
        :param Y: 绕 Y 轴旋转角度
        :param Z: 绕 Z 轴旋转角度
        :param order: 旋转顺序
        :return: 四元数
        """
        X = math.radians(X)
        Y = math.radians(Y)
        Z = math.radians(Z)

        cx = math.cos(X * 0.5)
        sx = math.sin(X * 0.5)
        cy = math.cos(Y * 0.5)
        sy = math.sin(Y * 0.5)
        cz = math.cos(Z * 0.5)
        sz = math.sin(Z * 0.5)

        qx = quaternion(cx, sx, 0, 0)
        qy = quaternion(cy, 0, sy, 0)
        qz = quaternion(cz, 0, 0, sz)

        if order == "XYZ":
            q = qz * qy * qx
        elif order == "XZY":
            q = qy * qz * qx
        elif order == "YXZ":
            q = qz * qx * qy
        elif order == "YZX":
            q = qx * qz * qy
        elif order == "ZXY":
            q = qy * qx * qz
        elif order == "ZYX":
            q = qx * qy * qz

        return q

    @staticmethod
    def from_to(p1: vector, p2: vector) -> "quaternion":
        """
        从 p1 到 p2 构建四元数
        :param p1: 起点
        :param p2: 终点
        :return: 四元数
        """
        u = p1.hat
        v = p2.hat
        axis = u.cross(v)
        axis_norm = axis.mag

        if axis_norm < 1e-6:
            if u.dot(v) > 0:
                # 无旋转
                return quaternion(1, 0, 0, 0)
            else:
                # 任意轴转 180 度
                return quaternion(0, 0, 0, 1)

        axis = axis.hat
        angle = math.acos(np.clip(u.dot(v), -1.0, 1.0))
        half_angle = angle * 0.5
        w = math.cos(half_angle)
        xyz = axis * math.sin(half_angle)
        return quaternion(w, xyz.x, xyz.y, xyz.z)

    @staticmethod
    def slerp(
        q1: "quaternion", q2: "quaternion", t1: float, t2: float, t: float
    ) -> "quaternion":
        """
        球面线性插值
        :param q1: 起始四元数
        :param q2: 终止四元数
        :param t1: 起始时间
        :param t2: 终止时间
        :param t: 目标时间
        :return: 四元数
        """
        d = quaternion.dot(q1, q2)
        if d < 0:
            q2 = -q2
            d = -d

        # 避免数值误差
        d = min(d, 1.0)
        d = max(d, 0.0)
        theta = math.acos(d)

        # 插值计算
        sin_theta = math.sin(theta)
        if sin_theta < 1e-6:
            return q1

        t = (t - t1) / (t2 - t1)
        k0 = math.sin((1 - t) * theta) / sin_theta
        k1 = math.sin(t * theta) / sin_theta
        return (q1 * k0 + q2 * k1).normalize()

    @staticmethod
    def sphere_to_quaternion(a: float, b: float, c: float) -> "quaternion":
        """
        四维球面点转四元数
        q = (cos(a),
            cos(a) sin(b) sin(c),
            sin(a) sin(b) sin(c),
            cos(b) sin(c))
        a, b in [0, pi], c in [0, 2*pi]
        :param a: 球面坐标系的 a 轴
        :param b: 球面坐标系的 b 轴
        :param c: 球面坐标系的 c 轴
        :return: 四元数
        """
        x = math.cos(a) * math.sin(b) * math.sin(c)
        y = math.sin(a) * math.sin(b) * math.sin(c)
        z = math.cos(b) * math.sin(c)
        w = math.cos(c)
        return quaternion(w, x, y, z)

    @staticmethod
    def quaternion_to_sphere(q: "quaternion") -> vector:
        """
        四元数转四维球面点
        :param q: 四元数
        :return: 球面坐标系的 a, b, c 轴
        """
        a = math.atan2(q.y, q.x)
        a = a if a >= 0 else -a
        b = math.atan2((1 if q.y > 0 else -1) * math.sqrt(q.x**2 + q.y**2), q.z)
        b = b if b >= 0 else -b
        c = math.acos(q.w)
        return vector(a, b, c)

    @staticmethod
    def average(
        quaternions: List["quaternion"],
        weights: List[float] | np.ndarray[Tuple[int], np.dtype[np.float64]],
    ) -> "quaternion":
        """
        四元数平均，注意单个四元数平均会有误差
        :param quaternions: 四元数列表
        :param weights: 权重列表
        :return: 平均四元数
        """
        if not weights:
            weights = np.ones(len(quaternions)) / len(quaternions)

        # 构建对称矩阵 M = Σ(weight * q.T @ q)
        M = np.zeros((4, 4))
        for q, w in zip(quaternions, weights):
            nq = None
            if type(q) is quaternion:
                nq = np.array([q.w, q.x, q.y, q.z])
            else:
                nq = np.array(q)
            M += w * np.outer(nq, nq)

        # 计算 M 的最大特征值对应的特征向量（即平均四元数）
        eigenvalues, eigenvectors = eigh(M)
        avg_quat = eigenvectors[:, np.argmax(eigenvalues)]

        return quaternion(avg_quat[0], avg_quat[1], avg_quat[2], avg_quat[3])

    @staticmethod
    def dot(p: "quaternion", q: "quaternion") -> float:
        """
        四元数点积
        :param p: 四元数
        :param q: 四元数
        :return: 点积
        """
        return p.w * q.w + p.x * q.x + p.y * q.y + p.z * q.z

    def conjugate(self) -> "quaternion":
        """
        四元数共轭
        :return: 共轭四元数
        """
        return quaternion(self.w, -self.x, -self.y, -self.z)

    def magnitude2(self) -> float:
        return self.w**2 + self.x**2 + self.y**2 + self.z**2

    def normalize(self) -> "quaternion":
        """
        归一化四元数
        :return: 归一化后的四元数
        """
        norm = self.magnitude2()
        return quaternion(self.w / norm, self.x / norm, self.y / norm, self.z / norm)

    def to_euler(self, order: str = "ZXY") -> vector:
        """
        四元数转欧拉角
        :param order: 旋转顺序
        :return: 绕 X 轴旋转角度, 绕 Y 轴旋转角度, 绕 Z 轴旋转角度
        """
        w, x, y, z = self.w, self.x, self.y, self.z
        sx2 = 2 * (w * x - y * z)
        sx2 = np.clip(sx2, -1, 1)

        # 需要考虑 3 种情况：euler_x = 90, euler_y = 0, euler_z = 0
        euler_x = math.asin(sx2) / 2
        sx = math.sin(euler_x)
        euler_y = (
            0
            if abs(w * y + z * x) < 1e-6
            else math.atan2(x**2 + y**2 - sx**2, w * y + z * x)
        )
        euler_z = (
            0
            if abs(w * z + y * x) < 1e-6
            else math.atan2(x**2 + z**2 - sx**2, w * z + y * x)
        )

        return (
            vector(math.degrees(euler_x), math.degrees(euler_y), math.degrees(euler_z))
            * 2
        )

    def to_euler_unity(self, order: str = "ZXY") -> vector:
        """
        四元数转 Unity 引擎的欧拉角
        :param order: 旋转顺序
        :return: 绕 X 轴旋转角度, 绕 Y 轴旋转角度, 绕 Z 轴旋转角度
        """
        w, x, y, z = self.w, self.x, self.y, self.z
        sqw = w * w
        sqx = x * x
        sqy = y * y
        sqz = z * z

        # Yaw Z 轴
        yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (sqy + sqz))
        # Pitch Y 轴
        pitch = math.asin(2 * (w * y - z * x))
        # Roll X 轴
        roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (sqx + sqy))

        # 处理万向节死锁
        threshold = 0.99999
        if abs(pitch) > threshold:
            if pitch > 0:
                yaw = math.atan2(x, z)
                roll = 0
            else:
                yaw = -math.atan2(-x, z)
                roll = 0

        return vector(math.degrees(roll), math.degrees(pitch), math.degrees(yaw))

    def to_array(self) -> List[float]:
        """
        四元数转数组
        :return: 数组 [w, x, y, z]
        """
        return [self.w, self.x, self.y, self.z]

    def __add__(self, other: "quaternion") -> "quaternion":
        return quaternion(
            self.w + other.w, self.x + other.x, self.y + other.y, self.z + other.z
        )

    def __mul__(
        self, other: Union["quaternion", vector, float]
    ) -> Union["quaternion", vector]:
        """
        四元数乘法
        :param other: 四元数或向量或标量
        :return: 四元数或向量
        """
        if type(other) is quaternion:
            v1 = vector(self.x, self.y, self.z)
            v2 = vector(other.x, other.y, other.z)
            w = self.w * other.w - v1.dot(v2)
            v = self.w * v2 + other.w * v1 + v1.cross(v2)
            return quaternion(w, v.x, v.y, v.z)
        elif type(other) is vector:
            q = quaternion(0, other.x, other.y, other.z)
            q = self * q * self.conjugate()
            return vector(q.x, q.y, q.z)
        elif isinstance(other, (float, int)):
            return quaternion(
                self.w * other, self.x * other, self.y * other, self.z * other
            )
        return NotImplemented

    def __neg__(self) -> "quaternion":
        return quaternion(-self.w, -self.x, -self.y, -self.z)

    def __repr__(self) -> str:
        return f"({self.x}, {self.y}, {self.z}, {self.w})"


class anim_config:
    """
    动画配置类
    """

    def __init__(self, config_path: str):
        """
        解析配置文件
        获得配置文件中的所有动画文件路径
        :param config_path: 配置文件路径
        """
        anim_path_json: Dict[str, Any] = json.load(open(config_path, "r"))
        root: str = anim_path_json["paths"]["root"]
        anim_dirs: List[str] = anim_path_json["paths"]["dirs"]
        anim_files: List[str] = anim_path_json["paths"]["files"]

        self.files = []
        for file in anim_files:
            self.files.append(root + file)

        # 递归搜索目录
        for dir in anim_dirs:
            dir = root + dir
            for file in Path(dir).glob("**/*" + anim_path_json["paths"]["suffix"]):
                self.files.append(str(file))

        self.anim_data: Dict[str, Any] = {}
        self.bones: List[str] = anim_path_json["bones"]
        self.step: float = anim_path_json["step"]

    def parse_data(self) -> None:
        """
        解析 .anim .json 动画数据并保存在 anim_data 中
        :return: None
        """
        with ThreadPoolExecutor() as executor:
            self.anim_data = dict(
                executor.map(anim_config.__parse_file_data, self.files)
            )

    def parse_and_save_data(self, prefix: str) -> None:
        """
        解析 .anim .json 并保存动画数据为 .json 文件
        :param prefix: 保存路径前缀
        :return: None
        """
        bound_parse = partial(anim_config.__parse_and_save_file_data, prefix=prefix)
        with ThreadPoolExecutor() as executor:
            executor.map(bound_parse, self.files)

    def extract_rotation_array(self) -> Dict[str, List[List[float]]]:
        """
        提取 Pose 骨骼旋转数据到数组
        :return: 骨骼数据字典，具有格式
        data = {
            "Bip_L_Thigh": [
                [1, 0, 0, 0],
                [0.9999999, 0, 0, 0.0000001],
                ...
            ],
            "Bip_R_Thigh": [
                [1, 0, 0, 0],
                [0.9999999, 0, 0, 0.0000001],
                ...
            ],
            ...
        }
        """
        step = self.step
        pose = anim_bone.generate_template_pose()
        anim_data = (data for data in self.anim_data.values())

        # 标记需要提取的骨骼
        data: Dict[str, List[List[float]]] = {}
        anim_pose: Dict[str, "anim_bone"] = {}
        for bone_name in self.bones:
            data[bone_name] = []
            anim_pose[bone_name] = pose[bone_name]

        for anim in anim_data:
            anim_bone.bind_anim_curves(anim, anim_pose)
            for bone in anim_pose.values():
                data[bone.name] += bone.extract_data_array(step)
                print(
                    f"{bone.name} extract {len(data[bone.name])} frames from {anim['AnimationClip']['m_Name']}"
                )

        return data

    @staticmethod
    def extract_rotation_target_array(data_file: str) -> Dict[str, List[List[float]]]:
        """
        提取目标骨骼旋转位置数据到数组
        :param data_file: 数据文件路径
        :return: 骨骼数据字典，具有格式
        """
        data = np.load(data_file)
        rotation_data: Dict[str, np.ndarray[Tuple[int, int], np.dtype[np.float64]]] = {
            k: v for k, v in data.items()
        }

        x = vector(1, 0, 0)
        target_data: Dict[str, List[List[float]]] = {}
        for key, rotations in rotation_data.items():
            target_data[key] = []
            for r in rotations:
                q = quaternion(r[0], r[1], r[2], r[3])
                p = q * x
                target_data[key].append([p.x, p.y, p.z])

        return target_data

    @staticmethod
    def __parse_file_data(filename: str) -> Tuple[str, Any]:
        """
        解析 .anim .json 动画数据
        :param filename: 动画文件路径
        :return: 动画数据
        """
        path = Path(filename)
        name = path.stem

        data = None
        if path.suffix == ".anim":
            data = anim_config.__parse_anim_data(filename)
        elif path.suffix == ".json":
            data = anim_config.__parse_json_data(filename)
        print(f"Parse {name} success.")
        return (name, data)

    @staticmethod
    def __parse_and_save_file_data(filename: str, prefix: str) -> None:
        """
        解析 .anim .json 并保存动画数据为 .json 文件
        :param filename: 动画文件路径
        :param prefix: 保存路径前缀
        :return: None
        """
        path = Path(filename)
        name = path.stem
        parent_name = path.parent.name

        data = None
        if path.suffix == ".anim":
            data = anim_config.__parse_anim_data(filename)
        elif path.suffix == ".json":
            data = anim_config.__parse_json_data(filename)
        print(f"Parse {parent_name}-{name} success.")

        with open(
            f"{prefix}{parent_name}-{name}.json",
            "w",
            encoding="utf-8",
        ) as f:
            json.dump(data, f, indent=4, ensure_ascii=False)
            print(f"Save {parent_name}-{name} success.")

    @staticmethod
    def __parse_anim_data(filename: str) -> Any:
        """
        解析 .anim 文件
        :param filename: 动画文件路径
        :return: 动画数据
        """
        with open(filename, "r", encoding="utf-8") as file:
            yaml_content = []
            for line in file:
                if "AnimationClip:" in line:
                    yaml_content.append(line)
                    yaml_content.extend(file)  # 剩余行直接追加
                    break

            # 解析 YAML 部分
            try:
                return yaml.safe_load("".join(yaml_content))
            except yaml.YAMLError as e:
                print(f"YAML解析错误: {e}")
            return None

    @staticmethod
    def __parse_json_data(filename: str) -> Any:
        """
        解析 JSON 文件
        :param filename: JSON 文件路径
        :return: JSON 数据
        """
        with open(filename, "r", encoding="utf-8") as file:
            try:
                return json.load(file)
            except json.JSONDecodeError as e:
                print(f"JSON解析错误: {e}")
            return None


class anim_curve:
    """
    动画曲线类
    """

    def __init__(
        self, curves: List[Callable[[float], quaternion | vector]], knots: List[float]
    ):
        """
        动画曲线
        :param curves: 分段曲线
        :param knots: 节点
        """
        self.curves = curves
        self.knots = knots

    def evaluate(self, time: float) -> quaternion | vector:
        """
        计算曲线值
        :param time: 时间
        :return: 曲线值
        """
        # 注意曲线比 knots 数量少
        index = bisect.bisect_left(self.knots, time) - 1
        index = max(index, 0)
        index = min(index, len(self.knots) - 2)
        return self.curves[index](time)


class anim_transform:
    """
    动画变换类
    """

    def __init__(
        self,
        pos: vector = vector(0, 0, 0),
        rotation: quaternion = quaternion(),
        scale: vector = vector(1, 1, 1),
    ):
        """
        动画变换
        :param pos: 位置
        :param rotation: 旋转
        :param scale: 缩放
        """
        self.pos = pos
        self.rotation = rotation
        self.scale = scale

    def to_dict(self) -> Dict[str, Any]:
        """
        转换为字典
        :return: 字典
        """
        return {
            "LocalScale": {"x": self.scale.x, "y": self.scale.y, "z": self.scale.z},
            "Location": {"x": self.pos.x, "y": self.pos.y, "z": self.pos.z},
            "Rotation": {
                "x": self.rotation.x,
                "y": self.rotation.y,
                "z": self.rotation.z,
                "w": self.rotation.w,
            },
        }

    @staticmethod
    def target(
        x: np.ndarray[Tuple[int], np.dtype[np.float64]], axis: vector = vector(1, 0, 0)
    ) -> vector:
        """
        将旋转数据转换为四元数
        :param x: 旋转数据
        :param axis: 轴
        :return: 四元数
        """
        if x.shape[0] == 3:
            return vector(x[0], x[1], x[2])
        elif x.shape[0] == 4:
            return quaternion(x[0], x[1], x[2], x[3]) * axis

    @staticmethod
    def scaling(
        x: np.ndarray[Tuple[int], np.dtype[np.float64]], axis: vector = vector(1, 0, 0)
    ) -> float:
        """
        计算缩放因子
        :param center: 中心点
        :return: 缩放因子
        """
        apose = quaternion.A_pose() * vector(1, 0, 0)
        center = anim_transform.target(x, axis)
        return math.degrees(apose.diff_angle(center)) / 2


class anim_bone:
    """
    骨骼类
    """

    enable_position_curve = True
    enable_rotation_curve = True
    enable_scale_curve = True

    def __init__(
        self,
        name: str,
        transform: anim_transform = anim_transform(),
        parent: Union["anim_bone", None] = None,
    ):
        """
        动画骨骼
        :param name: 骨骼名称
        :param transform: 初始变换
        :param parent: 父骨骼
        """
        self.name = name
        self.local_transform = transform
        self.world_transform = anim_transform()

        self.parent = parent
        self.children: Dict[str, "anim_bone"] = {}

        self.position_curve: anim_curve | None = None
        self.rotation_curve: anim_curve | None = None
        self.scale_curve: anim_curve | None = None

        self.anim_name = "none"
        self.anim_state = "none"
        self.anim_time = 0.0
        self.current_time = 0.0

        self.sphere: sphere | None = None
        self.children_arrows: Dict[anim_bone, arrow] = {}

        # 处理父级
        if parent:
            parent.children[name] = self

    def fullname(self) -> str:
        """
        获得骨骼全名
        :return: 骨骼全名
        """
        if self.parent:
            return self.parent.fullname() + "/" + self.name
        else:
            return self.name

    def debug_print(self) -> None:
        """
        打印骨骼信息
        :return: None
        """
        print(f"Bone: {self.fullname()}")
        print(
            f"  Local Transform: {self.local_transform.pos}, {self.local_transform.rotation}"
        )
        print(
            f"  World Transform: {self.world_transform.pos}, {self.world_transform.rotation}"
        )

        for name, child in self.children.items():
            child.debug_print()

    def extract_data_array(self, step: float) -> List[List[float]]:
        """
        保存骨骼数据到数组
        :param step: 时间步长
        :return: 骨骼数据数组
        """
        data = []
        for i in range(floor(self.anim_time / step)):
            self.update_time(step)
            self.update_local_transform()
            q = self.local_transform.rotation
            data.append([q.w, q.x, q.y, q.z])

        return data

    def show(
        self,
        sphere_color: color = color.red,
        arrow_color: color = color.green,
        recursive: bool = True,
    ) -> None:
        """
        显示骨骼
        :param sphere_color: 球体颜色
        :param arrow_color: 骨骼连线颜色
        :param recursive: 是否递归显示
        :return: None
        """
        pos = self.world_transform.pos
        if not self.sphere:
            self.sphere = sphere(pos=pos, radius=0.01, color=sphere_color)
        else:
            self.sphere.pos = pos
            self.sphere.color = sphere_color
            self.sphere.visible = True

        for child in self.children.values():
            if recursive:
                child.show(sphere_color, arrow_color, recursive)

            child_pos = child.world_transform.pos
            if not self.children_arrows.get(child):
                self.children_arrows[child] = arrow(
                    pos=pos,
                    axis=child_pos - pos,
                    shaftwidth=0.01,
                    color=arrow_color,
                )
            else:
                self.children_arrows[child].pos = pos
                self.children_arrows[child].axis = child_pos - pos
                self.children_arrows[child].color = arrow_color
                self.children_arrows[child].visible = True

    def hide(self, recursive: bool = True) -> None:
        """
        隐藏骨骼
        :param recursive: 是否递归隐藏
        :return: None
        """
        if not self.sphere:
            return

        self.sphere.visible = False
        for child in self.children.values():
            if recursive:
                child.hide(recursive)
            if self.children_arrows.get(child):
                self.children_arrows[child].visible = False

    def update_time(self, dt: float) -> None:
        """
        更新时间
        :param dt: 时间步长
        :return: None
        """
        self.current_time += dt
        if self.current_time > self.anim_time:
            self.current_time -= self.anim_time
            self.anim_state = "replay"

        for child in self.children.values():
            child.update_time(dt)

    def update_local_transform(self) -> None:
        """
        更新本地变换
        :return: None
        """
        t = self.current_time
        if anim_bone.enable_position_curve and self.position_curve:
            self.local_transform.pos = self.position_curve.evaluate(t)

        if anim_bone.enable_rotation_curve and self.rotation_curve:
            self.local_transform.rotation = self.rotation_curve.evaluate(t).normalize()

        if anim_bone.enable_scale_curve and self.scale_curve:
            self.local_transform.scale = self.scale_curve.evaluate(t)

        for name, child in self.children.items():
            child.update_local_transform()

    def update_world_transform(
        self, parent_bone: Union["anim_bone", None] = None
    ) -> None:
        """
        更新世界变换
        :param parent_bone: 父骨骼
        :return: None
        """
        if not parent_bone:
            self.world_transform = self.local_transform
        else:
            element_wise_multiply: Callable[[vector, vector], vector] = (
                lambda v1, v2: vector(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z)
            )

            self.world_transform.pos = (
                parent_bone.world_transform.pos
                + element_wise_multiply(
                    parent_bone.world_transform.rotation * self.local_transform.pos,
                    parent_bone.world_transform.scale,
                )
            )
            self.world_transform.rotation = (
                parent_bone.world_transform.rotation * self.local_transform.rotation
            )
            self.world_transform.scale = element_wise_multiply(
                self.local_transform.scale, parent_bone.world_transform.scale
            )

        for child in self.children.values():
            child.update_world_transform(self)

    @staticmethod
    def bind_anim_curves(
        anim: Dict[str, Any] | None, pose: Dict[str, "anim_bone"]
    ) -> None:
        """
        绑定动画曲线
        :param anim: 动画数据
        :param pose: 骨骼数据
        """
        # 用于避免动画数组迭代得到空值
        if not anim:
            return

        position_data = anim["AnimationClip"]["m_PositionCurves"]
        rotation_data = anim["AnimationClip"]["m_RotationCurves"]
        scale_data = anim["AnimationClip"]["m_ScaleCurves"]

        position_curves_data = {}
        for item in position_data:
            name = item["path"].rsplit("/", 1)[-1]
            if name in pose:
                position_curves_data[name] = item["curve"]["m_Curve"]

        rotation_curves_data = {}
        for item in rotation_data:
            name = item["path"].rsplit("/", 1)[-1]
            if name in pose:
                rotation_curves_data[name] = item["curve"]["m_Curve"]

        scale_curves_data = {}
        for item in scale_data:
            name = item["path"].rsplit("/", 1)[-1]
            if name in pose:
                scale_curves_data[name] = item["curve"]["m_Curve"]

        # 初始化
        for bone in pose.values():
            bone.anim_name = anim["AnimationClip"]["m_Name"]
            bone.anim_state = "none"
            bone.anim_time = 0
            bone.current_time = 0

        for bone_name, data in position_curves_data.items():
            curve, t = anim_bone.interpolate_position_curve(data)
            pose[bone_name].position_curve = curve
            pose[bone_name].anim_state = "play"
            pose[bone_name].anim_time = max(t, pose[bone_name].anim_time)
            print(
                f"Bind {pose[bone_name].anim_name} position curve to {pose[bone_name].fullname()}. Anim time: {t}"
            )

        for bone_name, data in rotation_curves_data.items():
            curve, t = anim_bone.interpolate_rotation_curve(data)
            pose[bone_name].rotation_curve = curve
            pose[bone_name].anim_state = "play"
            pose[bone_name].anim_time = max(t, pose[bone_name].anim_time)
            print(
                f"Bind {pose[bone_name].anim_name} rotation curve to {pose[bone_name].fullname()}. Anim time: {t}"
            )

        for bone_name, data in scale_curves_data.items():
            curve, t = anim_bone.interpolate_scale_curve(data)
            pose[bone_name].scale_curve = curve
            pose[bone_name].anim_state = "play"
            pose[bone_name].anim_time = max(t, pose[bone_name].anim_time)
            print(
                f"Bind {pose[bone_name].anim_name} scale curve to {pose[bone_name].fullname()}. Anim time: {t}"
            )

    @staticmethod
    def interpolate_position_curve(data) -> Tuple[anim_curve, float]:
        curves = []
        knots = []
        for i in range(len(data) - 1):
            t = np.array(
                [
                    data[i]["time"],
                    data[i + 1]["time"],
                ]
            )
            vx = np.array(
                [
                    data[i]["value"]["x"],
                    data[i + 1]["value"]["x"],
                ]
            )
            vy = np.array(
                [
                    data[i]["value"]["y"],
                    data[i + 1]["value"]["y"],
                ]
            )
            vz = np.array(
                [
                    data[i]["value"]["z"],
                    data[i + 1]["value"]["z"],
                ]
            )
            cx = CubicSpline(
                t,
                vx,
                bc_type=(
                    (1, data[i]["outSlope"]["x"]),
                    (1, data[i + 1]["inSlope"]["x"]),
                ),
            )
            cy = CubicSpline(
                t,
                vy,
                bc_type=(
                    (1, data[i]["outSlope"]["y"]),
                    (1, data[i + 1]["inSlope"]["y"]),
                ),
            )
            cz = CubicSpline(
                t,
                vz,
                bc_type=(
                    (1, data[i]["outSlope"]["z"]),
                    (1, data[i + 1]["inSlope"]["z"]),
                ),
            )
            # 注意 cx=cx 用于捕获值而非默认捕获的引用
            curves.append(lambda t, cx=cx, cy=cy, cz=cz: vector(cx(t), cy(t), cz(t)))
            knots.append(data[i]["time"])
        knots.append(data[-1]["time"])

        position_curve = anim_curve(curves, knots)
        anim_time = knots[-1]
        return position_curve, anim_time

    @staticmethod
    def interpolate_rotation_curve(data) -> Tuple[anim_curve, float]:
        curves = []
        knots = []
        for i in range(len(data) - 1):
            t = np.array(
                [
                    data[i]["time"],
                    data[i + 1]["time"],
                ]
            )
            qw = np.array(
                [
                    data[i]["value"]["w"],
                    data[i + 1]["value"]["w"],
                ]
            )
            qx = np.array(
                [
                    data[i]["value"]["x"],
                    data[i + 1]["value"]["x"],
                ]
            )
            qy = np.array(
                [
                    data[i]["value"]["y"],
                    data[i + 1]["value"]["y"],
                ]
            )
            qz = np.array(
                [
                    data[i]["value"]["z"],
                    data[i + 1]["value"]["z"],
                ]
            )
            qw_spline = CubicSpline(
                t,
                qw,
                bc_type=(
                    (1, data[i]["outSlope"]["w"]),
                    (1, data[i + 1]["inSlope"]["w"]),
                ),
            )
            qx_spline = CubicSpline(
                t,
                qx,
                bc_type=(
                    (1, data[i]["outSlope"]["x"]),
                    (1, data[i + 1]["inSlope"]["x"]),
                ),
            )
            qy_spline = CubicSpline(
                t,
                qy,
                bc_type=(
                    (1, data[i]["outSlope"]["y"]),
                    (1, data[i + 1]["inSlope"]["y"]),
                ),
            )
            qz_spline = CubicSpline(
                t,
                qz,
                bc_type=(
                    (1, data[i]["outSlope"]["z"]),
                    (1, data[i + 1]["inSlope"]["z"]),
                ),
            )
            curves.append(
                lambda t, qw_spline=qw_spline, qx_spline=qx_spline, qy_spline=qy_spline, qz_spline=qz_spline: quaternion(
                    qw_spline(t), qx_spline(t), qy_spline(t), qz_spline(t)
                )
            )
            knots.append(data[i]["time"])
        knots.append(data[-1]["time"])

        rotation_curve = anim_curve(curves, knots)
        anim_time = knots[-1]
        return rotation_curve, anim_time

    @staticmethod
    def interpolate_scale_curve(data) -> Tuple[anim_curve, float]:
        curves = []
        knots = []
        for i in range(len(data) - 1):
            t = np.array(
                [
                    data[i]["time"],
                    data[i + 1]["time"],
                ]
            )
            sx = np.array(
                [
                    data[i]["value"]["x"],
                    data[i + 1]["value"]["x"],
                ]
            )
            sy = np.array(
                [
                    data[i]["value"]["y"],
                    data[i + 1]["value"]["y"],
                ]
            )
            sz = np.array(
                [
                    data[i]["value"]["z"],
                    data[i + 1]["value"]["z"],
                ]
            )
            sx_spline = CubicSpline(
                t,
                sx,
                bc_type=(
                    (1, data[i]["outSlope"]["x"]),
                    (1, data[i + 1]["inSlope"]["x"]),
                ),
            )
            sy_spline = CubicSpline(
                t,
                sy,
                bc_type=(
                    (1, data[i]["outSlope"]["y"]),
                    (1, data[i + 1]["inSlope"]["y"]),
                ),
            )
            sz_spline = CubicSpline(
                t,
                sz,
                bc_type=(
                    (1, data[i]["outSlope"]["z"]),
                    (1, data[i + 1]["inSlope"]["z"]),
                ),
            )
            curves.append(
                lambda t, sx_spline=sx_spline, sy_spline=sy_spline, sz_spline=sz_spline: vector(
                    sx_spline(t), sy_spline(t), sz_spline(t)
                )
            )
            knots.append(data[i]["time"])
        knots.append(data[-1]["time"])

        scale_curve = anim_curve(curves, knots)
        anim_time = knots[-1]
        return scale_curve, anim_time

    @staticmethod
    def generate_template_pose() -> Dict[str, "anim_bone"]:
        """
        生成模板 Pose
        :return: 骨骼字典
        """
        root = anim_bone("Root")

        bip = anim_bone(
            "Bip",
            anim_transform(
                vector(0.154, 1.973, -0.001),
                quaternion.euler(-86.02901, -32.981, 126.434),
            ),
            root,
        )

        pelvis = anim_bone(
            "Bip_Pelvis",
            anim_transform(rotation=quaternion.euler(-91.853, 34.634990, 50.612)),
            bip,
        )

        spine = anim_bone(
            "Bip_Spine",
            anim_transform(
                vector(-0.1500264, 0.08291407, -0.00199312),
                quaternion.euler(-15.219, -4.607, 0.9860001),
            ),
            pelvis,
        )

        # 上半身
        spine1 = anim_bone(
            "Bip_Spine1",
            anim_transform(
                vector(-0.248, 0, 0),
                quaternion.euler(1.984, -2.069, 13.873),
            ),
            spine,
        )

        spine2 = anim_bone(
            "Bip_Spine2",
            anim_transform(
                vector(-0.167, 0, 0),
                quaternion.euler(-2.307, -3.891, -0.38),
            ),
            spine1,
        )

        neck = anim_bone(
            "Bip_Neck",
            anim_transform(
                vector(-0.351, -0.13, 0),
                quaternion.euler(13.621, -2.285, -13.234),
            ),
            spine2,
        )

        head = anim_bone(
            "Bip_Head",
            anim_transform(
                vector(-0.103, 0, 0),
                quaternion.euler(4.781, 7.602, 0.663),
            ),
            neck,
        )

        # 下半身
        left_thigh = anim_bone(
            "Bip_L_Thigh",
            anim_transform(
                vector(0.1602764, -0.1213105, 0.1198534),
                quaternion.euler(-6.824, 169.172, -11.542),
            ),
            spine,
        )

        right_thigh = anim_bone(
            "Bip_R_Thigh",
            anim_transform(
                vector(0.1364194, -0.03854813, -0.1828084),
                quaternion.euler(-15.506, 174.789, -7.913001),
            ),
            spine,
        )

        left_calf = anim_bone(
            "Bip_L_Calf",
            anim_transform(vector(-0.8397716, 0, 0), quaternion.euler(0, 0, 22.189)),
            left_thigh,
        )

        right_calf = anim_bone(
            "Bip_R_Calf",
            anim_transform(vector(-0.8397717, 0, 0), quaternion.euler(0, 0, 11.881)),
            right_thigh,
        )

        left_foot = anim_bone(
            "Bip_L_Foot",
            anim_transform(
                vector(-0.8735464, 0, 0), quaternion.euler(0.215, 5.389, -10.285)
            ),
            left_calf,
        )

        right_foot = anim_bone(
            "Bip_R_Foot",
            anim_transform(
                vector(-0.8735467, 0, 0), quaternion.euler(0.9630001, 3.324, -3.144)
            ),
            right_calf,
        )

        left_toe = anim_bone(
            "Bip_L_Toe0",
            anim_transform(
                vector(-0.234295, 0.2220116, 0),
                quaternion.euler(1.088, 3.296, -92.18401),
            ),
            left_foot,
        )

        right_toe = anim_bone(
            "Bip_R_Toe0",
            anim_transform(
                vector(-0.2342951, 0.2220117, 0),
                quaternion.euler(0.117, -0.854, -92.895),
            ),
            right_foot,
        )

        return {
            "Root": root,
            "Bip": bip,
            "Bip_Pelvis": pelvis,
            "Bip_Spine": spine,
            "Bip_Spine1": spine1,
            "Bip_Spine2": spine2,
            "Bip_Neck": neck,
            "Bip_Head": head,
            "Bip_L_Thigh": left_thigh,
            "Bip_R_Thigh": right_thigh,
            "Bip_L_Calf": left_calf,
            "Bip_R_Calf": right_calf,
            "Bip_L_Foot": left_foot,
            "Bip_R_Foot": right_foot,
            "Bip_L_Toe0": left_toe,
            "Bip_R_Toe0": right_toe,
        }


class anim_pose_driver_datas:
    def __init__(self, asset_file: str):
        """
        读取 Asset 文件
        """
        self.filename = asset_file
        self.header = []
        with open(asset_file, "r", encoding="utf-8") as file:
            yaml_content = []
            for line in file:
                if "MonoBehaviour:" in line:
                    yaml_content.append(line)
                    yaml_content.extend(file)  # 剩余行直接追加
                    break
                else:
                    self.header.append(line)

            # 解析 YAML 部分
            try:
                self.data = yaml.safe_load("".join(yaml_content))
            except yaml.YAMLError as e:
                print(f"YAML解析错误: {e}")

    def save_asset(self, asset_file: str) -> None:
        """
        保存 Asset 文件
        :param asset_file: 文件路径
        :return: None
        """
        with open(asset_file, "w", encoding="utf-8") as file:
            file.write("".join(self.header))
            file.write(yaml.dump(self.data))

    def save(self) -> None:
        """
        保存 Asset 文件
        :return: None
        """
        self.save_asset(self.filename)

    def init_target_data(self, bone_names: List[str]) -> None:
        """
        创建源骨骼
        :param bone_names: 骨骼名称列表
        :return: None
        """
        self.data["MonoBehaviour"]["SourceBones"] = bone_names
        self.data["MonoBehaviour"]["RBFParams"]["DistanceType"] = 1
        self.data["MonoBehaviour"]["RBFParams"]["Radius"] = 1

    def create_target_data(
        self, scale: float, bone_names: List[str], transforms: List[anim_transform]
    ) -> None:
        """
        创建目标数据
        :param scale: 缩放因子（角度均方差）
        :param bone_names: 骨骼名称列表
        :param transforms: 骨骼变换列表
        :return: None
        """
        posedata = {}
        posedata["Scale"] = scale
        posedata["BoneNames"] = [b for b in bone_names]
        posedata["BoneTransforms"] = [t.to_dict() for t in transforms]
        self.data["MonoBehaviour"]["PoseTargetDatas"].append(posedata)


class anim_distance:
    @staticmethod
    def quat_cosine_dist(
        x: np.ndarray[Tuple[int], np.dtype[np.float64]],
        y: np.ndarray[Tuple[int], np.dtype[np.float64]],
    ) -> float:
        """
        余弦距离：四元数的夹角
        :param x: 四元数
        :param y: 四元数
        :return: 余弦距离
        """
        d = np.dot(x, y)
        d = np.clip(d, -1, 1)
        return 0.5 * np.acos(2 * d**2 - 1)

    @staticmethod
    def quat_target_chord_dist(
        x: np.ndarray[Tuple[int], np.dtype[np.float64]],
        y: np.ndarray[Tuple[int], np.dtype[np.float64]],
    ) -> float:
        """
        目标距离：四元数作用于 x 轴后的向量距离
        :param x: 四元数
        :param y: 四元数
        :return: 目标距离
        """
        q1 = quaternion(x[0], x[1], x[2], x[3])
        q2 = quaternion(y[0], y[1], y[2], y[3])
        v1 = q1 * vector(1, 0, 0)
        v2 = q2 * vector(1, 0, 0)
        return (v1 - v2).mag

    @staticmethod
    def quat_target_angle_dist(
        x: np.ndarray[Tuple[int], np.dtype[np.float64]],
        y: np.ndarray[Tuple[int], np.dtype[np.float64]],
    ) -> float:
        """
        目标距离：四元数作用于 x 轴后的向量距离
        :param x: 四元数
        :param y: 四元数
        :return: 目标距离
        """
        q1 = quaternion(x[0], x[1], x[2], x[3])
        q2 = quaternion(y[0], y[1], y[2], y[3])
        v1 = q1 * vector(1, 0, 0)
        v2 = q2 * vector(1, 0, 0)
        return math.acos(np.clip(v1.dot(v2), -1, 1))

    @staticmethod
    def target_chord_dist(
        x: np.ndarray[Tuple[int], np.dtype[np.float64]],
        y: np.ndarray[Tuple[int], np.dtype[np.float64]],
    ) -> float:
        """
        目标距离：向量距离
        :param x: 向量
        :param y: 向量
        :return: 目标距离
        """
        return np.sqrt(np.square(np.array(x) - np.array(y)).sum())

    @staticmethod
    def target_angle_dist(
        x: np.ndarray[Tuple[int], np.dtype[np.float64]],
        y: np.ndarray[Tuple[int], np.dtype[np.float64]],
    ) -> float:
        """
        目标距离：向量距离
        :param x: 向量
        :param y: 向量
        :return: 目标距离
        """
        return math.acos(np.clip((np.array(x) * np.array(y)).sum(), -1, 1))

    @staticmethod
    def dist_func_sum_cross_combine(
        dist_func: Distance_Func,
        combine_num: int,
        single_num: int,
    ) -> Distance_Func:
        """
        根据距离函数、组合数、单个元素长度返回组合距离函数
        :param dist_func: 距离函数
        :param combine_num: 组合数
        :param single_num: 单个元素长度
        :return: 组合距离函数
        """
        n = combine_num
        m = single_num
        return (
            lambda x, y: 5
            * np.square(
                np.array(
                    [
                        dist_func(x[i * m : i * m + m], y[i * m : i * m + m])
                        for i in range(n)
                    ]
                ).sum()
            )
            - 4
            * np.square(
                np.array(
                    [
                        dist_func(x[i * m : i * m + m], y[i * m : i * m + m])
                        for i in range(n)
                    ]
                )
            ).sum()
        )

    @staticmethod
    def dist_func_sum_square_combine(
        dist_func: Distance_Func,
        combine_num: int,
        single_num: int,
    ) -> Distance_Func:
        """
        根据距离函数、组合数、单个元素长度返回组合距离函数
        :param dist_func: 距离函数
        :param combine_num: 组合数
        :param single_num: 单个元素长度
        :return: 组合距离函数
        """
        n = combine_num
        m = single_num
        return lambda x, y: np.square(
            np.array(
                [
                    dist_func(x[i * m : i * m + m], y[i * m : i * m + m])
                    for i in range(n)
                ]
            ).sum()
        )

    @staticmethod
    def dist_func_sqrt_combine(
        dist_func: Distance_Func,
        combine_num: int,
        single_num: int,
    ) -> Distance_Func:
        """
        根据距离函数、组合数、单个元素长度返回组合距离函数
        :param dist_func: 距离函数
        :param combine_num: 组合数
        :param single_num: 单个元素长度
        :return: 组合距离函数
        """
        n = combine_num
        m = single_num
        return lambda x, y: np.sqrt(
            np.square(
                np.array(
                    [
                        dist_func(x[i * m : i * m + m], y[i * m : i * m + m])
                        for i in range(n)
                    ]
                )
            ).sum()
        )

    @staticmethod
    def dist_func_square_combine(
        dist_func: Distance_Func,
        combine_num: int,
        single_num: int,
    ) -> Distance_Func:
        """
        根据距离函数、组合数、单个元素长度返回组合距离函数
        :param dist_func: 距离函数
        :param combine_num: 组合数
        :param single_num: 单个元素长度
        :return: 组合距离函数
        """
        n = combine_num
        m = single_num
        return lambda x, y: np.square(
            np.array(
                [
                    dist_func(x[i * m : i * m + m], y[i * m : i * m + m])
                    for i in range(n)
                ]
            )
        ).sum()

    @staticmethod
    def dist_func_min_combine(
        dist_func: Distance_Func,
        combine_num: int,
        single_num: int,
    ) -> Distance_Func:
        """
        根据距离函数、组合数、单个元素长度返回组合距离函数
        :param dist_func: 距离函数
        :param combine_num: 组合数
        :param single_num: 单个元素长度
        :return: 组合距离函数
        """
        n = combine_num
        m = single_num
        return lambda x, y: np.array(
            [dist_func(x[i * m : i * m + m], y[i * m : i * m + m]) for i in range(n)]
        ).min()

    @staticmethod
    def dist_func_max_combine(
        dist_func: Distance_Func,
        combine_num: int,
        single_num: int,
    ) -> Distance_Func:
        """
        根据距离函数、组合数、单个元素长度返回组合距离函数
        :param dist_func: 距离函数
        :param combine_num: 组合数
        :param single_num: 单个元素长度
        :return: 组合距离函数
        """
        n = combine_num
        m = single_num
        return lambda x, y: np.array(
            [dist_func(x[i * m : i * m + m], y[i * m : i * m + m]) for i in range(n)]
        ).max()

    @staticmethod
    def dist_func_euclid_combine(
        dist_func: Distance_Func,
        combine_num: int,
        single_num: int,
    ) -> Distance_Func:
        """
        根据距离函数、组合数、单个元素长度返回组合距离函数
        :param dist_func: 距离函数
        :param combine_num: 组合数
        :param single_num: 单个元素长度
        :return: 组合距离函数
        """
        n = combine_num
        m = single_num
        return lambda x, y: np.square(np.array(x) - np.array(y)).sum()


class anim_filter:
    """
    动画数据过滤工具类
    """

    def __init__(self, poses: List[List[float]]):
        self.poses = np.array(poses)

    def filter(
        self,
        X: np.ndarray[Tuple[int, int], np.dtype[np.float64]],
        dist_func: Distance_Func,
        threshold: float,
    ) -> Tuple[
        np.ndarray[Tuple[int, int], np.dtype[np.float64]],
        np.ndarray[Tuple[int, int, int], np.dtype[np.float64]],
    ]:
        """
        过滤数据
        :param X: 要过滤的数据
        :param dist_func: 距离函数
        :param threshold: 过滤阈值
        :return: 过滤后的数据
        """
        NX, Nfilter = [], []
        for i in range(len(self.poses)):
            apose = self.poses[i]
            NY = []
            for v in X:
                if dist_func(v, apose) > threshold:
                    NX.append(v)
                else:
                    NY.append(v)
            Nfilter.append(NY)

        return np.array(NX), np.array(Nfilter)


class anim_cluster:
    """
    动画数据聚类工具类，聚类返回数据格式
    data = {
        "Bip_L_Thigh": {
            "Clusters": [
                [1, 0, 0, 0],
                [0.9999999, 0, 0, 0.0000001],
                ...
            ],
            "Centers": [
                [1, 0, 0, 0],
                [0.9999999, 0, 0, 0.0000001],
                ...
            ],
        },
        "Bip_R_Thigh": {
            "Clusters": [
                [1, 0, 0, 0],
                [0.9999999, 0, 0, 0.0000001],
                ...
            ],
            "Centers": [
                [1, 0, 0, 0],
                [0.9999999, 0, 0, 0.0000001],
                ...
            ],
        },
        ...
    }
    """

    def __init__(self, data_file: str):
        # 如果运行出现 numpy.warnings.filterwarnings('ignore', r'All-NaN (slice|axis) encountered') 报错，就将其注释掉
        data = np.load(data_file)
        self.data: Dict[str, np.ndarray[Tuple[int, int], np.dtype[np.float64]]] = {
            k: v for k, v in data.items()
        }
        self.pyclustering_methods = {
            "KMedians": anim_cluster.__pyclustering_kmedians,
        }

    @staticmethod
    def merge_features(
        clusters: List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]],
        centers: np.ndarray[Tuple[int, int], np.dtype[np.float64]],
        divide_bitmap: List[List[int]],
        dist_func: Distance_Func,
        merge_threshold: float,
    ) -> Tuple[
        List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]],
        np.ndarray[Tuple[int, int], np.dtype[np.float64]],
        List[int],
    ]:
        """
        合并相似特征
        :param clusters: 聚类结果
        :param centers: 聚类中心
        :param dist_func: 距离函数
        :param merge_threshold: 合并阈值
        :return: 新的聚类结果、新的聚类中心
        """
        graph: List[List[int]] = [[0] * len(centers)] * len(centers)
        for center_i in range(len(centers)):
            for center_j in range(center_i + 1, len(centers)):
                if dist_func(centers[center_i], centers[center_j]) < merge_threshold:
                    graph[center_i][center_j] = graph[center_j][center_i] = 1

        def dfs(
            graph: List[List[int]],
            component: List[int],
            visited: List[bool],
            center_i: int,
        ) -> None:
            if not visited[center_i]:
                visited[center_i] = True
                component.append(center_i)
                for center_j in range(len(graph)):
                    if graph[center_i][center_j]:
                        dfs(graph, component, visited, center_j)

        components: List[List[int]] = []
        visited: List[bool] = [False] * len(centers)
        for center_i in range(len(centers)):
            if not visited[center_i]:
                component = []
                dfs(graph, component, visited, center_i)
                components.append(component)

        new_clusters: List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]] = []
        new_centers: List[np.ndarray[Tuple[int], np.dtype[np.float64]]] = []
        new_divide_bitmap: List[List[int]] = []
        merge_bitmaps: List[int] = []
        for component_i in range(len(components)):
            component = components[component_i]
            for center_i in component:
                new_clusters.append(clusters[center_i])
                new_centers.append(centers[center_i])
                merge_bitmaps.append(component_i)

                if divide_bitmap:
                    new_divide_bitmap.append(divide_bitmap[center_i])

        return new_clusters, np.array(new_centers), new_divide_bitmap, merge_bitmaps

    @staticmethod
    def divide_features(
        clusters: List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]],
        centers: np.ndarray[Tuple[int, int], np.dtype[np.float64]],
        dist_func: Distance_Func,
        divide_threshold: float,
    ) -> Tuple[
        List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]],
        np.ndarray[Tuple[int, int], np.dtype[np.float64]],
        List[List[int]],
    ]:
        """
        分割聚类结果
        :param clusters: 聚类结果
        :param centers: 聚类中心
        :param dist_func: 距离函数
        :param divide_threshold: 分割阈值
        :return: 新的聚类结果、新的聚类中心，每个点的过滤标记
        """
        new_cluster_array: List[List[np.ndarray[Tuple[int], np.dtype[np.float64]]]] = []
        divide_bitmap: List[List[int]] = []
        for cluster_index in range(len(centers)):
            center = centers[cluster_index]
            cluster = clusters[cluster_index]
            tup_cluster_bitmap = [
                (v, 1) if dist_func(v, center) > divide_threshold else (v, 0)
                for v in cluster
            ]
            new_cluster_array += [v[0] for v in tup_cluster_bitmap if v[1] == 1]
            divide_bitmap.append([v[1] for v in tup_cluster_bitmap])

        new_cluster = np.array(new_cluster_array)
        new_center = new_cluster.mean(axis=0)
        divide_bitmap.append([0] * len(new_cluster))

        clusters.append(np.array(new_cluster))
        centers = np.vstack((centers, new_center))

        return clusters, centers, divide_bitmap

    def pyclustering_cluster_combine(self, **args: Dict[str, Any]) -> None:
        """
        使用 pyclustering 库进行聚类并保存结果（联合数据），结果包括聚类中心和各个元素的聚类
        """
        method: str = args["method"]
        prefix: str = args["prefix"]
        num_clusters: int = args["num_clusters"]
        dist_func: Distance_Func = args["dist_func"]
        dist_func_combine: Distance_Func_Combine = args["dist_func_combine"]
        filter: anim_filter = args["filter"]
        filter_threshold: float = args["filter_threshold"]
        enable_merge: bool = args["enable_merge"]
        merge_threshold: float = args["merge_threshold"]
        enable_divide: bool = args["enable_divide"]
        divide_threshold: float = args["divide_threshold"]
        enable_recluster: bool = args["enable_recluster"]

        print(f"Pyclustering {method} combine clustering start.")

        # 联合数据
        bone_names: List[str] = list(self.data.keys())
        bone_datas: List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]] = list(
            self.data.values()
        )

        # 获得合并次数和合并前的元素长度
        combine_num, single_num = len(bone_datas), len(bone_datas[0][0])
        dist: Distance_Func = dist_func_combine(dist_func, combine_num, single_num)

        # 过滤数据，注意防止 dist 函数未被覆盖
        X: np.ndarray[Tuple[int, int], np.dtype[np.float64]] = np.concatenate(
            bone_datas, axis=1
        )
        X, Nfilter = filter.filter(X, dist, filter_threshold)

        metric = distance_metric(type_metric.USER_DEFINED, func=dist)
        labels, centers = self.pyclustering_methods[method](X, num_clusters, metric)

        # 提取聚类结果，使用 List 是因为聚类数量不同，不能对齐为高维 numpy 数组
        clusters: List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]] = [
            X[label] for label in labels
        ]

        # 划分聚类
        divide_bitmap: List[List[int]] = []
        if enable_divide:
            clusters, centers, divide_bitmap = anim_cluster.divide_features(
                clusters,
                centers,
                dist,
                divide_threshold,
            )

            # 重新聚类
            if enable_recluster:
                added_labels, added_centers = self.pyclustering_methods[method](
                    clusters[-1], num_clusters, metric
                )

                # 提取聚类结果，使用 List 是因为聚类数量不同，不能对齐为高维 numpy 数组
                added_clusters: List[
                    np.ndarray[Tuple[int, int], np.dtype[np.float64]]
                ] = [clusters[-1][label] for label in added_labels]

                # 移除最后一个聚类
                clusters.pop()
                divide_bitmap.pop()
                centers = np.delete(centers, -1, axis=0)

                # 并入新的聚类
                for cluster in added_clusters:
                    clusters.append(cluster)
                    divide_bitmap.append([1] * len(cluster))
                centers = np.vstack((centers, added_centers))

        # 合并相似特征，只调整顺序
        merge_bitmap: List[int] = []
        if enable_merge:
            clusters, centers, divide_bitmap, merge_bitmap = (
                anim_cluster.merge_features(
                    clusters,
                    centers,
                    divide_bitmap,
                    dist,
                    merge_threshold,
                )
            )

        # 保存结果
        data: Dict[str, Dict[str, Any]] = {}
        for bone_index in range(len(bone_names)):
            range_start = bone_index * single_num
            range_end = range_start + single_num

            # 提取这一部分分量的聚类结果
            clustered_data = [
                [v[range_start:range_end] for v in cluster] for cluster in clusters
            ]
            center_data = [center[range_start:range_end] for center in centers]

            merge_bitmap_data = merge_bitmap[:]
            divide_bipmap_data = divide_bitmap[:]

            # 补充过滤部分的聚类结果
            for filter_index in range(len(Nfilter)):
                if len(Nfilter[filter_index]) > 0:
                    clustered_data.append(
                        Nfilter[filter_index][:, range_start:range_end]
                    )
                    center_data.append(
                        filter.poses[filter_index][range_start:range_end]
                    )

                    if merge_bitmap:
                        merge_bitmap_data.append(merge_bitmap_data[-1] + 1)

                    if divide_bitmap:
                        divide_bipmap_data.append([1] * len(Nfilter[filter_index]))

            data[bone_names[bone_index]] = {
                "Clusters": clustered_data,
                "Centers": center_data,
                "MergeBitmap": merge_bitmap_data,
                "DivideBitmap": divide_bipmap_data,
            }

        print(f"Pyclustering {method} clustering {len(centers)} finished.")
        filename = anim_cluster.filename_combine(
            prefix, method, dist_func, dist_func_combine, num_clusters
        )
        np.savez(
            filename,
            **data,
        )
        print(f"Pyclustering {method} clustering data saved in {filename}.")

    @staticmethod
    def __pyclustering_kmedians(
        X: np.ndarray[Tuple[int, int], np.dtype[np.float64]],
        num_clusters: int,
        metric: distance_metric,
    ) -> Tuple[Any, Any]:
        centers = kmeans_plusplus_initializer(
            X, num_clusters, kmeans_plusplus_initializer.FARTHEST_CENTER_CANDIDATE
        ).initialize()

        kmedians_instance = kmedians(X, centers, metric=metric)
        kmedians_instance.process()
        return kmedians_instance.get_clusters(), kmedians_instance.get_medians()

    @staticmethod
    def filename_combine(
        prefix: str,
        method: str,
        dist_func: Distance_Func,
        dist_func_combine: Distance_Func_Combine,
        num_clusters: int,
    ) -> str:
        """
        生成联合数据保存文件名
        :param method: 聚类方法
        :param dist_func: 距离函数
        :param num_clusters: 聚类数量
        :return: 文件名
        """
        return f"{prefix}{method}_{dist_func.__name__}_{dist_func_combine.__name__}_{num_clusters}.npz"


class anim_cluster_analyzer:
    """
    动画聚类数据分析工具类
    """

    color_palettes = [
        "#FF0000",  # 红
        "#00FF00",  # 绿
        "#FFFF00",  # 黄
        "#800080",  # 紫
        "#FFA500",  # 橙
        "#0000FF",  # 蓝
        "#FFC0CB",  # 粉红
        "#00FFFF",  # 青
        "#FF0097",  # 玫红
        "#98FF98",  # 薄荷绿
    ]

    bone_colors = {
        "Bip_L_Thigh": color.orange,
        "Bip_R_Thigh": color.purple,
        "Bip_L_Thigh_Bitmap": color.cyan,
        "Bip_R_Thigh_Bitmap": color.white,
    }

    bone_offsets = {"Bip_L_Thigh": vector(-2, 0, 0), "Bip_R_Thigh": vector(2, 0, 0)}

    def __init__(
        self,
        prefix: str,
        method: str,
        dist_func: Distance_Func,
        dist_func_combine: Distance_Func_Combine,
        num_clusters: int,
    ):
        """
        :param method: 聚类方法
        :param dist_func: 距离函数
        :param prefix: 保存文件名前缀
        :param combine: 是否使用联合数据
        """
        self.combine = dist_func_combine
        filename = anim_cluster.filename_combine(
            prefix,
            method,
            dist_func,
            dist_func_combine,
            num_clusters,
        )
        data = np.load(filename, allow_pickle=True)
        self.data = {
            key: (data[key].item() if isinstance(data[key], np.ndarray) else data[key])
            for key in data.files
        }

        # 组合距离函数
        self.combine_num = len(self.data)
        self.single_num = len(self.data[list(self.data.keys())[0]]["Centers"][0])
        self.dist: Distance_Func = self.combine(
            dist_func, self.combine_num, self.single_num
        )
        self.paused = False
        self.show_index = 0

    def extract_pose_driver_datas(
        self, bones: Dict[str, vector], pose_driver_datas: anim_pose_driver_datas
    ):
        data: Dict[str, Any] = self.data
        bone_names: List[str] = list(bones.keys())

        # 合并特征
        merged_data = {}
        for bone_name in bone_names:
            components = [[0]]
            prev_index = 0
            merge_bitmap = data[bone_name]["MergeBitmap"]
            for merge_index in range(len(merge_bitmap) - 1):
                if merge_bitmap[merge_index + 1] != prev_index:
                    components.append([])
                    prev_index = merge_bitmap[merge_index + 1]
                components[-1].append(merge_index + 1)

            clusters: List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]] = []
            centers: List[np.ndarray[Tuple[int], np.dtype[np.float64]]] = []
            bone_data = data[bone_name]

            for component in components:
                cluster = np.concatenate(
                    [bone_data["Clusters"][i] for i in component], axis=0
                )
                center = np.mean([bone_data["Centers"][i] for i in component], axis=0)
                clusters.append(cluster)
                centers.append(center)

            merged_data[bone_name] = {}
            merged_data[bone_name]["Clusters"] = clusters
            merged_data[bone_name]["Centers"] = centers

        scales: List[float] = []
        transforms: List[List[anim_transform]] = []

        for center_index in range(len(merged_data[bone_names[0]]["Centers"])):
            combine_angles: List[List[float]] = []
            combine_transforms: List[anim_transform] = []
            for bone_name, trans in bones.items():
                bone_data = merged_data[bone_name]
                cluster = bone_data["Clusters"][center_index]
                center = anim_transform.target(
                    bone_data["Centers"][center_index], vector(1, 0, 0)
                )

                # 使用最近点估计法获得旋转角度
                # dist = [self.dist(v, bone_data["Centers"][center_index]) for v in cluster]
                # nearest_index = np.argmin(dist)

                combine_transforms.append(
                    anim_transform(
                        pos=trans["position"],
                        rotation=quaternion.from_to(trans["rotation"] * vector(1, 0, 0), center)
                        * trans["rotation"],
                    )
                )

                # angles = []
                # for point_index in range(len(cluster)):
                #     pos = anim_transform.target(cluster[point_index], vector(1, 0, 0))
                #     angles.append(math.degrees(center.diff_angle(pos)))

                # combine_angles.append(angles)
            transforms.append(combine_transforms)

            # 计算影响半径
            combine_radius: List[float] = [
                anim_transform.scaling(merged_data[bone_name]["Centers"][center_index])
                for bone_name in bone_names
            ]
            radius = np.array(combine_radius).mean()

            # radius = 0
            # for point_index in range(len(combine_angles[0])):
            #     err = 0
            #     for bone_index in range(len(combine_angles)):
            #         err += combine_angles[bone_index][point_index] ** 2
            #     radius += math.sqrt(err)
            # radius /= len(combine_angles[0])
            scales.append(float(radius))

        for scale, trans in zip(scales, transforms):
            pose_driver_datas.create_target_data(scale, bone_names, trans)

    def prepare_cluster_objects(
        self, pose: Dict[str, anim_bone]
    ) -> Tuple[List[List[arrow]], List[List[List[sphere]]], List[List[List[arrow]]]]:
        """
        生成聚类对象
        :param pose: 动画骨架
        :return: 聚类中心箭头、聚类球体、各个聚类之间的连线
        """
        data: Dict[str, Dict[str, Any]] = self.data
        bip = pose["Bip"]
        bip_spline = pose["Bip_Spine"]

        cluster_combinearraws: List[List[List[arrow]]] = []
        cluster_spheres: List[List[List[sphere]]] = []
        center_arrows: List[List[arrow]] = []

        # 生成每个聚类的球体，对应左右腿
        cached_points: List[List[vector]] = []
        for key, value in data.items():
            bone_arrows: List[List[arrow]] = []
            bone_spheres: List[List[sphere]] = []
            bone_points: List[List[vector]] = []
            for cluster_index in range(len(value["Clusters"])):
                cluster = value["Clusters"][cluster_index]
                spheres = []
                arrows = []
                points = []
                for point_index in range(len(cluster)):
                    point_color = (
                        anim_cluster_analyzer.bone_colors[key]
                        if (
                            value["DivideBitmap"] == []
                            or value["DivideBitmap"][cluster_index][point_index] == 0
                        )
                        else anim_cluster_analyzer.bone_colors[f"{key}_Bitmap"]
                    )
                    pos = -anim_transform.target(cluster[point_index], vector(1, 0, 0))
                    spheres.append(
                        sphere(
                            pos=bip_spline.world_transform.rotation * pos
                            + bip.world_transform.pos
                            + anim_cluster_analyzer.bone_offsets[key],
                            radius=0.01,
                            color=point_color,
                            visible=False,
                        )
                    )
                    if cached_points == []:
                        points.append(
                            bip_spline.world_transform.rotation * pos
                            + bip.world_transform.pos
                            + vector(0, 0, -3)
                        )
                        arrows.append(
                            arrow(
                                pos=bip.world_transform.pos + vector(0, 0, -3),
                                axis=bip_spline.world_transform.rotation * pos,
                                shaftwidth=0.005,
                                color=point_color,
                                visible=False,
                            )
                        )
                    else:
                        arrows.append(
                            arrow(
                                pos=cached_points[cluster_index][point_index],
                                axis=bip_spline.world_transform.rotation * pos,
                                shaftwidth=0.005,
                                color=point_color,
                                visible=False,
                            )
                        )
                bone_arrows.append(arrows)
                bone_spheres.append(spheres)
                bone_points.append(points)
            cluster_spheres.append(bone_spheres)
            cluster_combinearraws.append(bone_arrows)
            cached_points = bone_points

        for key, value in data.items():
            bone_arrows: List[arrow] = []
            for center in value["Centers"]:
                pos = anim_transform.target(np.array(center), vector(1, 0, 0))
                axis = bip_spline.world_transform.rotation * pos
                bone_arrows.append(
                    arrow(
                        pos=anim_cluster_analyzer.bone_offsets[key]
                        + bip.world_transform.pos,
                        axis=-axis,
                        shaftwidth=0.01,
                        color=anim_cluster_analyzer.bone_colors[key],
                        visible=False,
                    )
                )
            center_arrows.append(bone_arrows)

        return center_arrows, cluster_spheres, cluster_combinearraws

    def analyze_pose(
        self, width: int = 1600, height: int = 800, fps: int = 30, vis_time: int = 600
    ) -> None:
        """
        分析组合聚类的 Pose
        :param width: 画布宽度
        :param height: 画布高度
        :param fps: 帧率
        :param vis_time: 动画时长
        """
        data: Dict[str, Dict[str, Any]] = self.data
        bone_names: List[str] = [k for k in data.keys()]
        pose_data: List[Dict[str, Any]] = [v for k, v in data.items()]

        # 标准 pose
        pose = anim_bone.generate_template_pose()
        root = pose["Root"]
        bip = pose["Bip"]

        # 这里刷新是为了获得 bip 偏移值用来显示聚类球体
        root.update_world_transform()

        scene.width = width
        scene.height = height
        bone_info = label(
            pos=vector(10, height - 50, 0),
            text="Hello, VPython!",
            pixel_pos=True,
            align="left",
        )

        # 准备聚类数据
        center_arrows, cluster_spheres, cluster_combinearraws = (
            self.prepare_cluster_objects(pose)
        )

        # 绑定按键
        scene.bind("keydown", self.on_key)

        current_index = [0] * len(pose_data)
        current_time = time.time()
        start_time = time.time()
        while True:
            if self.paused:
                # start_time = time.time() - (current_time - start_time)
                # current_time = time.time()
                continue

            if current_time - start_time > vis_time:
                break

            # 立即暂停
            self.paused = True

            # 每秒切换一次聚类中心
            bone_info.text = "Current centers"
            for bone_index in range(len(pose_data)):
                # current_index[i] = floor(current_time - start_time) % len(bone_data)

                bone_data: np.ndarray[Tuple[int, int], np.dtype[np.float64]] = (
                    pose_data[bone_index]["Centers"]
                )
                current_index[bone_index] = self.show_index % len(bone_data)

                from_vec = vector(1, 0, 0)
                to_vec = anim_transform.target(
                    np.array(bone_data[current_index[bone_index]]), from_vec
                )
                pose[bone_names[bone_index]].local_transform.rotation = (
                    quaternion.from_to(from_vec, to_vec)
                )

                bone_info.text += (
                    f"\n{bone_names[bone_index]}: Center {current_index[bone_index]}"
                )

                if pose_data[bone_index]["MergeBitmap"]:
                    bone_info.text += f"\nMerge: {pose_data[bone_index]['MergeBitmap'][current_index[bone_index]]}"

            # 只显示当前聚类球体和转轴
            for bone_index in range(len(cluster_spheres)):
                for cluster_index in range(len(cluster_spheres[bone_index])):
                    visible = cluster_index == current_index[bone_index]
                    for obj in cluster_spheres[bone_index][cluster_index]:
                        obj.visible = visible
                    for obj in cluster_combinearraws[bone_index][cluster_index]:
                        obj.visible = visible

            for bone_index in range(len(center_arrows)):
                for cluster_index in range(len(center_arrows[bone_index])):
                    center_arrows[bone_index][cluster_index].visible = (
                        cluster_index == current_index[bone_index]
                    )

            root.update_time(time.time() - current_time)
            root.update_local_transform()
            root.update_world_transform()

            # bip_l_thigh_info.pos = pose["Bip_L_Thigh"].world_transform.pos
            # bip_r_thigh_info.pos = pose["Bip_R_Thigh"].world_transform.pos

            # 更新相机视点
            scene.center = bip.world_transform.pos
            bip.show()

            # 设置大腿颜色
            for bone in bone_names:
                pose[bone].show(arrow_color=anim_cluster_analyzer.bone_colors[bone])

            current_time = time.time()
            rate(fps)

    def analyze_cluster(
        self,
        bone_name: str,
        width: int = 1600,
        height: int = 800,
        fps: int = 30,
        vis_time: int = 600,
    ) -> None:
        """
        可视化聚类分布循环，用于骨骼聚类数据可视化
        :param bone_name: 骨骼名称
        :param width: 画布宽度
        :param height: 画布高度
        :param fps: 帧率
        :param vis_time: 动画时长
        """
        # A-Pose
        sphere(
            pos=quaternion.A_pose() * vector(1, 0, 0),
            radius=0.05,
            color=color.white,
        )

        x = cylinder(
            pos=vector(0, 0, 0), axis=vector(1, 0, 0), radius=0.01, color=color.red
        )
        y = cylinder(
            pos=vector(0, 0, 0), axis=vector(0, 1, 0), radius=0.01, color=color.green
        )
        z = cylinder(
            pos=vector(0, 0, 0), axis=vector(0, 0, 1), radius=0.01, color=color.blue
        )

        cluster_data: np.ndarray[Tuple[int, int, int], np.dtype[np.float64]] = (
            self.data[bone_name]["Clusters"]
        )
        centers: np.ndarray[Tuple[int, int], np.dtype[np.float64]] = self.data[
            bone_name
        ]["Centers"]

        # 绘制转轴
        for center_index in range(len(centers)):
            cluster_color = anim_cluster_analyzer.color_palettes[
                center_index % len(anim_cluster_analyzer.color_palettes)
            ]
            cluster_color = anim_cluster_analyzer.hex_to_rgb(cluster_color)

            # 转轴
            for v in cluster_data[center_index]:
                sphere(
                    pos=anim_transform.target(v),
                    radius=0.01,
                    color=cluster_color,
                )

            # 聚类中心
            center = anim_transform.target(centers[center_index])
            sphere(
                pos=center,
                radius=0.02,
                color=cluster_color,
            )

            label(pos=center, text=f"{bone_name}_{center_index}", color=color.white)

        scene.width = width
        scene.height = height
        current_time = time.time()
        start_time = time.time()
        while True:
            if current_time - start_time > vis_time:
                break
            current_time = time.time()
            rate(fps)

    def analyze_data(self, rows: int, cols: int) -> None:
        """
        分析数据聚类结果
        :param rows: 绘图行数
        :param cols: 绘图列数
        """
        bone_names: List[str] = list(self.data.keys())
        centers: List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]] = [
            v["Centers"] for v in self.data.values()
        ]
        clusters: List[np.ndarray[Tuple[int, int, int], np.dtype[np.float64]]] = [
            v["Clusters"] for v in self.data.values()
        ]

        for bone_index in range(len(bone_names)):
            for center_index in range(len(centers[bone_index])):
                pos = anim_transform.target(centers[bone_index][center_index])
                q = quaternion.from_to(vector(1, 0, 0), pos)
                print(f"{bone_names[bone_index]} center {center_index}: {q.to_euler()}")

        # 合并数据
        centers_combine: np.ndarray[Tuple[int, int], np.dtype[np.float64]] = (
            np.concatenate(centers, axis=1)
        )
        clusters_combine: List[np.ndarray[Tuple[int, int], np.dtype[np.float64]]] = []

        for center_index in range(len(centers_combine)):
            cluster = []
            for bone_index in range(len(bone_names)):
                cluster.append(clusters[bone_index][center_index])
            clusters_combine.append(np.concatenate(cluster, axis=1))

        # 计算距离
        dist_data: List[List[float]] = []
        for center_index in range(len(centers_combine)):
            dist_data.append([])
            for v in clusters_combine[center_index]:
                dist_data[-1].append(self.dist(centers_combine[center_index], v))

        # 绘制距离分布图
        fig, axes = plt.subplots(nrows=rows, ncols=cols, figsize=(15, 6))
        for center_index in range(len(dist_data)):
            axe = axes[center_index // cols, center_index % cols]
            axe.hist(
                dist_data[center_index],
                bins=30,
                alpha=0.7,
                color="blue",
                edgecolor="black",
            )
            axe.set_title(f"Center {center_index}")
            axe.set_xlabel("Distance")
            axe.set_ylabel("Frequency")

        plt.tight_layout()
        plt.show()

    def on_key(self, event) -> None:
        """
        按键事件
        """
        if event.key == " ":
            self.paused = not self.paused
        elif event.key == "p":
            self.show_index += 1
            self.paused = False
        elif event.key == "o":
            self.show_index -= 1
            self.paused = False

    @staticmethod
    def custom_anim(
        config_file: str,
        width: int = 1600,
        height: int = 800,
        fps: int = 30,
        vis_time: int = 600,
    ) -> None:
        """
        可视化动画循环
        :param config_file: 配置文件路径
        :param width: 画布宽度
        :param height: 画布高度
        :param fps: 帧率
        :param vis_time: 动画时长
        """
        config = anim_config(config_file)
        config.parse_data()

        pose = anim_bone.generate_template_pose()

        anim_data: Generator[Dict[str, Any], None, None] = (
            data for data in config.anim_data.values()
        )
        current_anim: Dict[str, Any] | None = next(anim_data, None)
        anim_bone.bind_anim_curves(current_anim, pose)

        scene.width = width
        scene.height = height
        anim_info = label(
            pos=vector(10, height - 20, 0),
            text="Hello, VPython!",
            pixel_pos=True,
            align="left",
        )

        current_time = time.time()
        start_time = time.time()
        while True:
            if current_time - start_time > vis_time:
                break

            if pose["Root"].anim_state == "replay":
                current_anim = next(anim_data, None)
                anim_bone.bind_anim_curves(current_anim, pose)

            anim_info.text = f"{pose["Root"].anim_name} | Time: {pose["Root"].current_time:.2f} / {pose["Root"].anim_time:.2f}"

            pose["Root"].update_time(time.time() - current_time)
            pose["Root"].update_local_transform()
            pose["Root"].update_world_transform()

            # 更新相机视点
            scene.center = pose["Bip"].world_transform.pos

            # 显示动画
            pose["Bip"].show()

            current_time = time.time()
            rate(fps)

    @staticmethod
    def custom_rotation_pose(
        data: Dict[str, Dict[str, List[List[float]]]],
        width: int = 1600,
        height: int = 800,
        fps: int = 30,
        vis_time: int = 600,
    ) -> None:
        """
        分析组合聚类的 Pose
        :param data: 自定义数据，应当具有格式
        poses = {
            "Bip_R_Thigh": {
                "Centers": [
                    quaternion.euler(0, 180, -90).to_array(),
                    quaternion.euler(0, 180, 0).to_array(),
                ],
            },
            "Bip_L_Thigh": {
                "Centers": [
                    quaternion.euler(0, 180, 0).to_array(),
                    quaternion.euler(0, 180, -90).to_array(),
                ],
            },
        }
        :param width: 画布宽度
        :param height: 画布高度
        :param fps: 帧率
        :param vis_time: 动画时长
        """
        bone_names = [k for k in data.keys()]
        pose_data = [v for k, v in data.items()]

        # 标准 pose
        pose = anim_bone.generate_template_pose()
        root = pose["Root"]
        bip = pose["Bip"]

        scene.width = width
        scene.height = height
        bone_info = label(
            pos=vector(10, height - 50, 0),
            text="Hello, VPython!",
            pixel_pos=True,
            align="left",
        )

        current_index = [0] * len(pose_data)
        current_time = time.time()
        start_time = time.time()
        while True:
            if current_time - start_time > vis_time:
                break

            # 每秒切换一次
            bone_info.text = "Current poses"
            for bone_index in range(len(pose_data)):
                center_data = pose_data[bone_index]["Centers"]
                current_index[bone_index] = floor(current_time - start_time) % len(
                    center_data
                )

                q = quaternion(
                    center_data[current_index[bone_index]][0],
                    center_data[current_index[bone_index]][1],
                    center_data[current_index[bone_index]][2],
                    center_data[current_index[bone_index]][3],
                )
                pose[bone_names[bone_index]].local_transform.rotation = q
                bone_info.text += f"\n{bone_names[bone_index]}: {q.to_euler()}, index: {current_index[bone_index]}"

            root.update_time(time.time() - current_time)
            root.update_local_transform()
            root.update_world_transform()

            # 更新相机视点
            scene.center = bip.world_transform.pos
            bip.show()

            current_time = time.time()
            rate(fps)

    @staticmethod
    def custom_anim_loop(
        width: int = 1600,
        height: int = 800,
        fps: int = 30,
        vis_time: int = 600,
        show_axis: bool = True,
    ) -> None:
        """
        自定义动画循环
        :param width: 画布宽度
        :param height: 画布高度
        :param fps: 帧率
        :param vis_time: 动画时长
        :param show_axis: 是否显示坐标轴
        """
        if show_axis:
            x = cylinder(
                pos=vector(0, 0, 0), axis=vector(1, 0, 0), radius=0.01, color=color.red
            )
            y = cylinder(
                pos=vector(0, 0, 0),
                axis=vector(0, 1, 0),
                radius=0.01,
                color=color.green,
            )
            z = cylinder(
                pos=vector(0, 0, 0), axis=vector(0, 0, 1), radius=0.01, color=color.blue
            )

        scene.width = width
        scene.height = height
        current_time = time.time()
        start_time = time.time()
        while True:
            if current_time - start_time > vis_time:
                break
            current_time = time.time()
            rate(fps)

    @staticmethod
    def hex_to_rgb(hex_code: str) -> vector:
        """
        将十六进制颜色码转换为 RGB 颜色
        :param hex_code: 十六进制颜色码
        :return: RGB 颜色
        """
        hex_code = hex_code.lstrip("#")
        r = int(hex_code[0:2], 16) / 255.0
        g = int(hex_code[2:4], 16) / 255.0
        b = int(hex_code[4:6], 16) / 255.0
        return vector(r, g, b)
